/*
 * svs_client.cpp
 *
 *  Simple client that connects to svs_node to get some images and to save them as rgb files
 *
 *  Open with ImageMagick: $> display -size WxH -depth 8 rgb:filename
 */

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <iostream>
#include <fstream>
#include <string>
#include <sstream>
#include <iomanip>

void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
	static int i = 0;
	std::stringstream ss;

	ss << "image_";
	ss.width(5);
	ss.fill('0');
	ss << i++ << ".rgb";
	std::cout << "Image #" << i << " received. Saving to " << ss.str() << std::endl;

	std::ofstream image_fd(ss.str().c_str());
	image_fd.write((const char*)&msg->data[0], msg->data.size());
	image_fd.close();
}

int main(int argc, char* argv[]){
	ros::init(argc, argv, "svs_client");
	ros::NodeHandle nh;
	image_transport::ImageTransport it(nh);

	if (argc < 2){
		std::cout << "Usage: " << argv[0] << " image_topic\n";
		return -1;
	}

	image_transport::Subscriber sub = it.subscribe(argv[1], 1, imageCallback);

	ros::spin();
	return 0;
}
